/*
 * File: ik6s.c
 *
 * MATLAB Coder version            : 4.1
 * C/C++ source code generated on  : 29-Jul-2020 11:48:39
 */

 /* Include Files */
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <time.h> 
//#include "rt_defines.h"
//#include "rt_nonfinite.h"
#include <string.h>
#include <rocr6_driver/ik6s.h>
//#include "sqrt.h"
//#include "mrdivide_helper.h"

#define RT_PI	3.1415926535897931

/* Function Declarations */
static double rt_atan2d_snf(double u0, double u1);

/* Function Definitions */

/*
 * Arguments    : double A[16]
 *                const double B[16]
 * Return Type  : void
 */
void mrdivide_helper(double A[16], const double B[16])
{
	double b_A[16];
	signed char ipiv[4];
	int j;
	int jA;
	int jj;
	int jp1j;
	int iy;
	int n;
	int k;
	int i1;
	int ix;
	double smax;
	int kBcol;
	double s;
	memcpy(&b_A[0], &B[0], sizeof(double) << 4);
	ipiv[0] = 1;
	ipiv[1] = 2;
	ipiv[2] = 3;
	ipiv[3] = 4;
	for (j = 0; j < 3; j++) {
		jA = j * 5;
		jj = j * 5;
		jp1j = jA + 2;
		n = 4 - j;
		iy = 0;
		ix = jA;
		smax = fabs(b_A[jA]);
		for (k = 2; k <= n; k++) {
			ix++;
			s = fabs(b_A[ix]);
			if (s > smax) {
				iy = k - 1;
				smax = s;
			}
		}

		if (b_A[jj + iy] != 0.0) {
			if (iy != 0) {
				iy += j;
				ipiv[j] = (signed char)(iy + 1);
				smax = b_A[j];
				b_A[j] = b_A[iy];
				b_A[iy] = smax;
				ix = j + 4;
				iy += 4;
				smax = b_A[ix];
				b_A[ix] = b_A[iy];
				b_A[iy] = smax;
				ix += 4;
				iy += 4;
				smax = b_A[ix];
				b_A[ix] = b_A[iy];
				b_A[iy] = smax;
				ix += 4;
				iy += 4;
				smax = b_A[ix];
				b_A[ix] = b_A[iy];
				b_A[iy] = smax;
			}

			i1 = (jj - j) + 4;
			for (kBcol = jp1j; kBcol <= i1; kBcol++) {
				b_A[kBcol - 1] /= b_A[jj];
			}
		}

		n = 2 - j;
		iy = jA + 4;
		jA = jj;
		for (kBcol = 0; kBcol <= n; kBcol++) {
			smax = b_A[iy];
			if (b_A[iy] != 0.0) {
				ix = jj + 1;
				i1 = jA + 6;
				k = (jA - j) + 8;
				for (jp1j = i1; jp1j <= k; jp1j++) {
					b_A[jp1j - 1] += b_A[ix] * -smax;
					ix++;
				}
			}

			iy += 4;
			jA += 4;
		}
	}

	for (j = 0; j < 4; j++) {
		jA = j << 2;
		iy = jA - 1;
		for (k = 0; k < j; k++) {
			kBcol = k << 2;
			smax = b_A[k + jA];
			if (smax != 0.0) {
				A[1 + iy] -= smax * A[kBcol];
				A[2 + iy] -= smax * A[kBcol + 1];
				A[3 + iy] -= smax * A[kBcol + 2];
				A[4 + iy] -= smax * A[kBcol + 3];
			}
		}

		smax = 1.0 / b_A[j + jA];
		A[1 + iy] *= smax;
		A[2 + iy] *= smax;
		A[3 + iy] *= smax;
		A[4 + iy] *= smax;
	}

	for (j = 3; j >= 0; j--) {
		jA = (j << 2) - 1;
		i1 = j + 2;
		for (k = i1; k < 5; k++) {
			kBcol = (k - 1) << 2;
			smax = b_A[k + jA];
			if (smax != 0.0) {
				A[1 + jA] -= smax * A[kBcol];
				A[2 + jA] -= smax * A[kBcol + 1];
				A[3 + jA] -= smax * A[kBcol + 2];
				A[4 + jA] -= smax * A[kBcol + 3];
			}
		}
	}

	for (iy = 2; iy >= 0; iy--) {
		if (ipiv[iy] != iy + 1) {
			kBcol = iy << 2;
			smax = A[kBcol];
			i1 = (ipiv[iy] - 1) << 2;
			A[kBcol] = A[i1];
			A[i1] = smax;
			jA = 1 + kBcol;
			smax = A[jA];
			k = 1 + i1;
			A[jA] = A[k];
			A[k] = smax;
			jA = 2 + kBcol;
			smax = A[jA];
			k = 2 + i1;
			A[jA] = A[k];
			A[k] = smax;
			kBcol += 3;
			smax = A[kBcol];
			i1 += 3;
			A[kBcol] = A[i1];
			A[i1] = smax;
		}
	}
}

/*
 * Arguments    : double *x
 * Return Type  : void
 */
void b_sqrt(double *x)
{
	*x = sqrt(*x);
}

/*
 * Arguments    : double u0
 *                double u1
 * Return Type  : double
 */
static double rt_atan2d_snf(double u0, double u1)
{
	double y;
	// int b_u0;
	// int b_u1;
	// if (rtIsNaN(u0) || rtIsNaN(u1)) {
	  // y = rtNaN;
	// } else if (rtIsInf(u0) && rtIsInf(u1)) {
	  // if (u0 > 0.0) {
		// b_u0 = 1;
	  // } else {
		// b_u0 = -1;
	  // }

	  // if (u1 > 0.0) {
		// b_u1 = 1;
	  // } else {
		// b_u1 = -1;
	  // }

	  // y = atan2(b_u0, b_u1);
	// } else 
	if (u1 == 0.0) {
		if (u0 > 0.0) {
			y = RT_PI / 2.0;
		}
		else if (u0 < 0.0) {
			y = -(RT_PI / 2.0);
		}
		else {
			y = 0.0;
		}
	}
	else {
		y = atan2(u0, u1);
	}

	return y;
}

/*
 *==========================================
 *		d        a        alpha    offset
 *==========================================
 *      0.144    0        0        0;
 *      0        0        pi/2     -pi/2;
 *      0        -0.264   0        0;
 *      0.106    -0.236   0        -pi/2;
 *      0.114    0        pi/2     0;
 *      0.067    0        -pi/2    0;
 *------------------------------------------
 * Arguments    : const double sp[6]
 *                const double dh[24]
 *                double itheta[48]
 * Return Type  : result
 */
int ik6s(const double sp[6], const double dh[24], double itheta[48])
{
	double ForJudgment;
	double C_idx_0;
	double T06[16];
	double B_idx_0;
	double theta23_idx_0;
	double S5_1;
	double C234_idx_0;
	double dv0[16];
	int k;
	double d0;
	double theta1_1;
	double theta1_2;
	double a_tmp;
	double theta234_idx_1;
	double theta5_1;
	double theta5_2;
	double a_tmp_tmp;
	double b_a_tmp_tmp;
	double b_a_tmp;
	double S5_3;
	double theta5_3;
	double d1;
	double theta5_4;
	double theta6_1;
	double B2_idx_0;
	double B1_idx_0;
	int i;
	double d2;
	int j;
	int i0;

	/* alpha=dh(:,1); */
	ForJudgment = cos(sp[4]);
	C_idx_0 = cos(sp[3]);
	T06[0] = ForJudgment * C_idx_0;
	B_idx_0 = sin(sp[3]);
	T06[4] = -ForJudgment * B_idx_0;
	theta23_idx_0 = sin(sp[4]);
	T06[8] = theta23_idx_0;
	T06[12] = sp[0];
	S5_1 = sin(sp[5]);
	C234_idx_0 = cos(sp[5]);
	T06[1] = S5_1 * theta23_idx_0 * C_idx_0 + C234_idx_0 * B_idx_0;
	T06[5] = -S5_1 * theta23_idx_0 * B_idx_0 + C234_idx_0 * C_idx_0;
	T06[9] = -sin(sp[5]) * ForJudgment;
	T06[13] = sp[1];
	T06[2] = -C234_idx_0 * theta23_idx_0 * C_idx_0 + S5_1 * B_idx_0;
	T06[6] = C234_idx_0 * theta23_idx_0 * B_idx_0 + S5_1 * C_idx_0;
	T06[10] = C234_idx_0 * ForJudgment;
	T06[14] = sp[2];
	dv0[1] = 0.0;
	dv0[5] = 1.0;
	dv0[9] = -0.0;
	dv0[13] = -0.0 * dh[5];
	dv0[2] = 0.0;
	dv0[6] = 0.0;
	dv0[10] = 1.0;
	dv0[14] = dh[5];
	T06[3] = 0.0;
	dv0[0] = 1.0;
	dv0[3] = 0.0;
	T06[7] = 0.0;
	dv0[4] = 0.0;
	dv0[7] = 0.0;
	T06[11] = 0.0;
	dv0[8] = 0.0;
	dv0[11] = 0.0;
	T06[15] = 1.0;
	dv0[12] = 0.0;
	dv0[15] = 1.0;
	mrdivide_helper(T06, dv0);
	memset(&itheta[0], 0, 48U * sizeof(double));
	k = 0;
	ForJudgment = (T06[12] * T06[12] + T06[13] * T06[13]) - dh[3] * dh[3];
	if (!(ForJudgment < -1.0E-6)) {
		if ((ForJudgment >= -1.0E-6) && (ForJudgment < 0.0)) {
			ForJudgment = 0.0;
		}

		d0 = ForJudgment;
		b_sqrt(&d0);
		C_idx_0 = rt_atan2d_snf(T06[13], T06[12]);
		theta1_1 = C_idx_0 - rt_atan2d_snf(-dh[3], d0);
		b_sqrt(&ForJudgment);
		theta1_2 = C_idx_0 - rt_atan2d_snf(-dh[3], -ForJudgment);
		C_idx_0 = sin(theta1_1);
		theta23_idx_0 = cos(theta1_1);
		C234_idx_0 = theta23_idx_0 * T06[1];
		a_tmp = -C_idx_0 * T06[0] + C234_idx_0;
		theta234_idx_1 = -C_idx_0 * T06[4] + theta23_idx_0 * T06[5];
		S5_1 = a_tmp * a_tmp + theta234_idx_1 * theta234_idx_1;
		b_sqrt(&S5_1);
		ForJudgment = C_idx_0 * T06[8] - theta23_idx_0 * T06[9];
		theta5_1 = rt_atan2d_snf(S5_1, ForJudgment);
		d0 = a_tmp * a_tmp + theta234_idx_1 * theta234_idx_1;
		b_sqrt(&d0);
		theta5_2 = rt_atan2d_snf(-d0, ForJudgment);
		a_tmp_tmp = sin(theta1_2);
		a_tmp = cos(theta1_2);
		b_a_tmp_tmp = a_tmp * T06[1];
		ForJudgment = -a_tmp_tmp * T06[0] + b_a_tmp_tmp;
		b_a_tmp = -a_tmp_tmp * T06[4] + a_tmp * T06[5];
		S5_3 = ForJudgment * ForJudgment + b_a_tmp * b_a_tmp;
		b_sqrt(&S5_3);
		B_idx_0 = a_tmp_tmp * T06[8] - a_tmp * T06[9];
		theta5_3 = rt_atan2d_snf(S5_3, B_idx_0);
		d1 = ForJudgment * ForJudgment + b_a_tmp * b_a_tmp;
		b_sqrt(&d1);
		theta5_4 = rt_atan2d_snf(-d1, B_idx_0);
		if (fabs(S5_1) > 1.0E-6) {
			theta6_1 = rt_atan2d_snf(theta234_idx_1 / S5_1, (C_idx_0 * T06[0] -
				C234_idx_0) / S5_1);
			ForJudgment = -T06[10] / S5_1;
			C234_idx_0 = -(theta23_idx_0 * T06[8] + C_idx_0 * T06[9]) / S5_1;
			S5_1 = rt_atan2d_snf(ForJudgment, C234_idx_0);
			B1_idx_0 = (theta23_idx_0 * T06[12] + C_idx_0 * T06[13]) - dh[4] *
				ForJudgment;
			B2_idx_0 = (T06[14] - dh[0]) + dh[4] * C234_idx_0;
			ForJudgment = -2.0 * B2_idx_0 * dh[8];
			B_idx_0 = 2.0 * B1_idx_0 * dh[8];
			C_idx_0 = ((B1_idx_0 * B1_idx_0 + B2_idx_0 * B2_idx_0) + dh[8] * dh[8]) -
				dh[9] * dh[9];
			d2 = (ForJudgment * ForJudgment + B_idx_0 * B_idx_0) - C_idx_0 * C_idx_0;
			if (d2 >= 0.0) {
				C234_idx_0 = d2;
				b_sqrt(&C234_idx_0);
				ForJudgment = rt_atan2d_snf(B_idx_0, ForJudgment);
				C234_idx_0 = ForJudgment - rt_atan2d_snf(C_idx_0, C234_idx_0);
				b_sqrt(&d2);
				C_idx_0 = ForJudgment - rt_atan2d_snf(C_idx_0, -d2);
				theta23_idx_0 = rt_atan2d_snf((B2_idx_0 - dh[8] * sin(C234_idx_0)) / dh
					[9], (B1_idx_0 - dh[8] * cos(C234_idx_0)) / dh[9]);
				ForJudgment = rt_atan2d_snf((B2_idx_0 - dh[8] * sin(C_idx_0)) / dh[9],
					(B1_idx_0 - dh[8] * cos(C_idx_0)) / dh[9]);
				itheta[0] = theta1_1;
				itheta[8] = C234_idx_0 - dh[19];
				itheta[16] = theta23_idx_0 - C234_idx_0;
				itheta[24] = (S5_1 - theta23_idx_0) - dh[21];
				itheta[32] = theta5_1;
				itheta[40] = theta6_1;
				itheta[1] = theta1_1;
				itheta[9] = C_idx_0 - dh[19];
				itheta[17] = ForJudgment - C_idx_0;
				itheta[25] = (S5_1 - ForJudgment) - dh[21];
				itheta[33] = theta5_1;
				itheta[41] = theta6_1;
				k = 2;
			}
		}

		if (fabs(-d0) > 1.0E-6) {
			theta6_1 = rt_atan2d_snf(theta234_idx_1 / -d0, (sin(theta1_1) * T06[0] -
				cos(theta1_1) * T06[1]) / -d0);
			ForJudgment = -T06[10] / -d0;
			C234_idx_0 = -(cos(theta1_1) * T06[8] + sin(theta1_1) * T06[9]) / -d0;
			theta234_idx_1 = rt_atan2d_snf(ForJudgment, C234_idx_0);
			theta23_idx_0 = (cos(theta1_1) * T06[12] + sin(theta1_1) * T06[13]) - dh[4]
				* ForJudgment;
			C_idx_0 = (T06[14] - dh[0]) + dh[4] * C234_idx_0;
			ForJudgment = -2.0 * C_idx_0 * dh[8];
			C234_idx_0 = 2.0 * theta23_idx_0 * dh[8];
			B2_idx_0 = ((theta23_idx_0 * theta23_idx_0 + C_idx_0 * C_idx_0) + dh[8] *
				dh[8]) - dh[9] * dh[9];
			d0 = (ForJudgment * ForJudgment + C234_idx_0 * C234_idx_0) - B2_idx_0 *
				B2_idx_0;
			if (d0 >= 0.0) {
				d2 = d0;
				b_sqrt(&d2);
				ForJudgment = rt_atan2d_snf(C234_idx_0, ForJudgment);
				B1_idx_0 = ForJudgment - rt_atan2d_snf(B2_idx_0, d2);
				b_sqrt(&d0);
				B_idx_0 = ForJudgment - rt_atan2d_snf(B2_idx_0, -d0);
				S5_1 = rt_atan2d_snf((C_idx_0 - dh[8] * sin(B1_idx_0)) / dh[9],
					(theta23_idx_0 - dh[8] * cos(B1_idx_0)) / dh[9]);
				ForJudgment = rt_atan2d_snf((C_idx_0 - dh[8] * sin(B_idx_0)) / dh[9],
					(theta23_idx_0 - dh[8] * cos(B_idx_0)) / dh[9]);
				itheta[k] = theta1_1;
				itheta[8 + k] = B1_idx_0 - dh[19];
				itheta[16 + k] = S5_1 - B1_idx_0;
				itheta[24 + k] = (theta234_idx_1 - S5_1) - dh[21];
				itheta[32 + k] = theta5_2;
				itheta[40 + k] = theta6_1;
				itheta[k + 1] = theta1_1;
				itheta[k + 9] = B_idx_0 - dh[19];
				itheta[k + 17] = ForJudgment - B_idx_0;
				itheta[k + 25] = (theta234_idx_1 - ForJudgment) - dh[21];
				itheta[k + 33] = theta5_2;
				itheta[k + 41] = theta6_1;
				k += 2;
			}
		}

		if (fabs(S5_3) > 1.0E-6) {
			B2_idx_0 = rt_atan2d_snf(b_a_tmp / S5_3, (a_tmp_tmp * T06[0] - b_a_tmp_tmp)
				/ S5_3);
			ForJudgment = -T06[10] / S5_3;
			C234_idx_0 = -(a_tmp * T06[8] + a_tmp_tmp * T06[9]) / S5_3;
			theta6_1 = rt_atan2d_snf(ForJudgment, C234_idx_0);
			B1_idx_0 = (a_tmp * T06[12] + a_tmp_tmp * T06[13]) - dh[4] * ForJudgment;
			C234_idx_0 = (T06[14] - dh[0]) + dh[4] * C234_idx_0;
			ForJudgment = -2.0 * C234_idx_0 * dh[8];
			S5_1 = 2.0 * B1_idx_0 * dh[8];
			C_idx_0 = ((B1_idx_0 * B1_idx_0 + C234_idx_0 * C234_idx_0) + dh[8] * dh[8])
				- dh[9] * dh[9];
			d0 = (ForJudgment * ForJudgment + S5_1 * S5_1) - C_idx_0 * C_idx_0;
			if (d0 >= 0.0) {
				d2 = d0;
				b_sqrt(&d2);
				ForJudgment = rt_atan2d_snf(S5_1, ForJudgment);
				theta23_idx_0 = ForJudgment - rt_atan2d_snf(C_idx_0, d2);
				b_sqrt(&d0);
				C_idx_0 = ForJudgment - rt_atan2d_snf(C_idx_0, -d0);
				B_idx_0 = rt_atan2d_snf((C234_idx_0 - dh[8] * sin(theta23_idx_0)) / dh[9],
					(B1_idx_0 - dh[8] * cos(theta23_idx_0)) / dh[9]);
				ForJudgment = rt_atan2d_snf((C234_idx_0 - dh[8] * sin(C_idx_0)) / dh[9],
					(B1_idx_0 - dh[8] * cos(C_idx_0)) / dh[9]);
				itheta[k] = theta1_2;
				itheta[8 + k] = theta23_idx_0 - dh[19];
				itheta[16 + k] = B_idx_0 - theta23_idx_0;
				itheta[24 + k] = (theta6_1 - B_idx_0) - dh[21];
				itheta[32 + k] = theta5_3;
				itheta[40 + k] = B2_idx_0;
				itheta[k + 1] = theta1_2;
				itheta[k + 9] = C_idx_0 - dh[19];
				itheta[k + 17] = ForJudgment - C_idx_0;
				itheta[k + 25] = (theta6_1 - ForJudgment) - dh[21];
				itheta[k + 33] = theta5_3;
				itheta[k + 41] = B2_idx_0;
				k += 2;
			}
		}

		if (fabs(-d1) > 1.0E-6) {
			theta6_1 = rt_atan2d_snf(b_a_tmp / -d1, (sin(theta1_2) * T06[0] - cos
			(theta1_2) * T06[1]) / -d1);
			ForJudgment = -T06[10] / -d1;
			C_idx_0 = -(cos(theta1_2) * T06[8] + sin(theta1_2) * T06[9]) / -d1;
			B2_idx_0 = rt_atan2d_snf(ForJudgment, C_idx_0);
			B1_idx_0 = (cos(theta1_2) * T06[12] + sin(theta1_2) * T06[13]) - dh[4] *
				ForJudgment;
			C234_idx_0 = (T06[14] - dh[0]) + dh[4] * C_idx_0;
			ForJudgment = -2.0 * C234_idx_0 * dh[8];
			S5_1 = 2.0 * B1_idx_0 * dh[8];
			theta23_idx_0 = ((B1_idx_0 * B1_idx_0 + C234_idx_0 * C234_idx_0) + dh[8] *
				dh[8]) - dh[9] * dh[9];
			d0 = (ForJudgment * ForJudgment + S5_1 * S5_1) - theta23_idx_0 *
				theta23_idx_0;
			if (d0 >= 0.0) {
				C_idx_0 = sqrt(d0);
				ForJudgment = rt_atan2d_snf(S5_1, ForJudgment);
				S5_1 = ForJudgment - rt_atan2d_snf(theta23_idx_0, C_idx_0);
				B_idx_0 = ForJudgment - rt_atan2d_snf(theta23_idx_0, -C_idx_0);
				theta23_idx_0 = rt_atan2d_snf((C234_idx_0 - dh[8] * sin(S5_1)) / dh[9],
					(B1_idx_0 - dh[8] * cos(S5_1)) / dh[9]);
				ForJudgment = rt_atan2d_snf((C234_idx_0 - dh[8] * sin(B_idx_0)) / dh[9],
					(B1_idx_0 - dh[8] * cos(B_idx_0)) / dh[9]);
				itheta[k] = theta1_2;
				itheta[8 + k] = S5_1 - dh[19];
				itheta[16 + k] = theta23_idx_0 - S5_1;
				itheta[24 + k] = (B2_idx_0 - theta23_idx_0) - dh[21];
				itheta[32 + k] = theta5_4;
				itheta[40 + k] = theta6_1;
				itheta[k + 1] = theta1_2;
				itheta[k + 9] = B_idx_0 - dh[19];
				itheta[k + 17] = ForJudgment - B_idx_0;
				itheta[k + 25] = (B2_idx_0 - ForJudgment) - dh[21];
				itheta[k + 33] = theta5_4;
				itheta[k + 41] = theta6_1;
				k += 2;
			}
		}

		if (k > 0) {
			for (i = 0; i < k; i++) {
				for (j = 0; j < 6; j++) {
					i0 = i + (j << 3);
					if (itheta[i0] <= -3.1415926535897931) {
						itheta[i0] += 6.2831853071795862;
					}
					else {
						if (itheta[i0] > 3.1415926535897931) {
							itheta[i0] -= 6.2831853071795862;

						}
					}
				}
			}
		}
		else {
			printf("奇异位置，无穷解\r\n");
			return 1;
		}
	}
	else {
		printf("超出工作空间，无法求解\r\n");
		return 2;
	}
	return 0;
}

#ifdef TEST
double itheta[48];
double sp[] = { 0.3,0.4,0.3,0,0,0 };
double dh[] =
{
	0.144,		0,			0,			0.106,			0.114,			0.067,
	0,			0,		   -0.264,	   -0.236,			0,				0,
	0,			RT_PI / 2,	0,			0,				RT_PI / 2,	   -RT_PI / 2,
	0,		   -RT_PI / 2,	0,		   -RT_PI / 2,		0,				0
};

int main(int argc, char* argv[])
{
	printf("\r\n--------\r\n");
        printf("x=%f  y=%f  z=%f  R=%f  P=%f  Y=%f\r\n",sp[0], sp[1], sp[2], sp[3], sp[4], sp[5]);
	printf("--------\r\n");
	ik6s(sp, dh, itheta);
	for (int i = 0; i < 8; i++)
	{
		printf("%d:	%4.4f    %4.4f    %4.4f    %4.4f    %4.4f    %4.4f\r\n", 
				i + 1, 
				itheta[i] * 180 / RT_PI, 
				itheta[i + 8] * 180 / RT_PI, 
			    itheta[i + 16] * 180 / RT_PI, 
			    itheta[i + 24] * 180 / RT_PI, 
				itheta[i + 32] * 180 / RT_PI, 
				itheta[i + 40] * 180 / RT_PI);
	}
	printf("--------\r\n");
}
#endif

/*
 * File trailer for ik6s.c
 *
 * [EOF]
 */
